feat(ekf_localizer): add covariance ellipse diagnostics #7708

SakodaShintaro · 2024-06-26T09:23:19Z

Description

I have added some diagnostics based on the covariance ellipse to the ekf_localizer. Its contents are the same as localization_error_monitor, but to avoid unnecessary pub/sub 50Hz odometry, I think it is better to publish the diagnostics in ekf_localizer.

I also think it makes sense in terms of modularity that ekf_localizer provides diagnostics for the publised PoseWithCovariance by itself.

This pull request just adds /diagnostics to ekf_localizer. In next another pull requests, I replace localization_error_monitor to ekf_localizer's /diagnostics.

How was this PR tested?

I have confirmed that logging_simulator works well.

In /diagnostics, ekf_localizer outputs the ellipse values, which is the same as localization_error_monitor.

Notes for reviewers

None.

Interface changes

Topic changes

Modifications

Version	Topic Type	Topic Name	Message Type	Description
Add	Pub	`/diagnostics`	`sensor_msgs/Image`	Added 6 statuses

cov_sllipse_long_axis_size
cov_sllipse_long_axis_warn_threshold
cov_sllipse_long_axis_error_threshold
cov_sllipse_lateral_direction_size
cov_sllipse_lateral_direction_warn_threshold
cov_sllipse_lateral_direction_error_threshold

ROS Parameter Changes

Additions and removals

Change type	Parameter Name	Type	Default Value	Description
Added	`ellipse_scale`	`double`	`3.0`	The scale factor to apply the error ellipse size
Added	`error_ellipse_size`	`double`	`1.5`	The long axis size of the error ellipse to trigger a ERROR state
Added	`warn_ellipse_size`	`double`	`1.2`	The long axis size of the error ellipse to trigger a WARN state
Added	`error_ellipse_size_lateral_direction`	`double`	`0.3`	The lateral direction size of the error ellipse to trigger a ERROR state
Added	`warn_ellipse_size_lateral_direction`	`double`	`0.25`	The lateral direction size of the error ellipse to trigger a WARN state

Effects on system behavior

None.

Signed-off-by: Shintaro Sakoda <[email protected]>

github-actions · 2024-06-26T09:23:41Z

Thank you for contributing to the Autoware project!

🚧 If your pull request is in progress, switch it to draft mode.

Please ensure:

You've checked our contribution guidelines.
Your PR follows our pull request guidelines.
All required CI checks pass before marking the PR ready for review.

codecov · 2024-06-26T09:30:58Z

Codecov Report

All modified and coverable lines are covered by tests ✅

Project coverage is 24.23%. Comparing base (3d849e9) to head (96eab49).
Report is 542 commits behind head on main.

Additional details and impacted files

@@             Coverage Diff             @@
##             main    #7708       +/-   ##
===========================================
+ Coverage   15.09%   24.23%    +9.14%     
===========================================
  Files        1967       67     -1900     
  Lines      135941     3268   -132673     
  Branches    42122      660    -41462     
===========================================
- Hits        20520      792    -19728     
+ Misses      92700     2323    -90377     
+ Partials    22721      153    -22568

Flag	Coverage Δ
differential	`24.23% <100.00%> (?)`
total	`?`

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

Signed-off-by: Shintaro Sakoda <[email protected]>

KYabuuchi

I have confirmed that ekf_localizer outputs diagnostics equivalent to localization_error_monitor. Also, the implementation seems fine to me. 🙆‍♂️

Could you update the diagnostics section on README? 🙏

Signed-off-by: Shintaro Sakoda <[email protected]>

SakodaShintaro · 2024-06-28T08:43:47Z

@KYabuuchi
UPDATED 🆙

Signed-off-by: Shintaro Sakoda <[email protected]>

KYabuuchi

Thank you for updating the README. Looks Good To me 🆗

…dation#7708) * Added ellipse diagnostics to ekf Signed-off-by: Shintaro Sakoda <[email protected]> * Removed an unnecessary parenthesis Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed to ellipse_scale Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed to ellipse_scale Signed-off-by: Shintaro Sakoda <[email protected]> * Updated ekf_diagnostics.png Signed-off-by: Shintaro Sakoda <[email protected]> * Added condition Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed "and" to "or" Signed-off-by: Shintaro Sakoda <[email protected]> --------- Signed-off-by: Shintaro Sakoda <[email protected]>

…dation#7708) * Added ellipse diagnostics to ekf Signed-off-by: Shintaro Sakoda <[email protected]> * Removed an unnecessary parenthesis Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed to ellipse_scale Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed to ellipse_scale Signed-off-by: Shintaro Sakoda <[email protected]> * Updated ekf_diagnostics.png Signed-off-by: Shintaro Sakoda <[email protected]> * Added condition Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed "and" to "or" Signed-off-by: Shintaro Sakoda <[email protected]> --------- Signed-off-by: Shintaro Sakoda <[email protected]> Signed-off-by: palas21 <[email protected]>

…dation#7708) * Added ellipse diagnostics to ekf Signed-off-by: Shintaro Sakoda <[email protected]> * Removed an unnecessary parenthesis Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed to ellipse_scale Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed to ellipse_scale Signed-off-by: Shintaro Sakoda <[email protected]> * Updated ekf_diagnostics.png Signed-off-by: Shintaro Sakoda <[email protected]> * Added condition Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed "and" to "or" Signed-off-by: Shintaro Sakoda <[email protected]> --------- Signed-off-by: Shintaro Sakoda <[email protected]>

* Added ellipse diagnostics to ekf Signed-off-by: Shintaro Sakoda <[email protected]> * Removed an unnecessary parenthesis Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed to ellipse_scale Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed to ellipse_scale Signed-off-by: Shintaro Sakoda <[email protected]> * Updated ekf_diagnostics.png Signed-off-by: Shintaro Sakoda <[email protected]> * Added condition Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed "and" to "or" Signed-off-by: Shintaro Sakoda <[email protected]> --------- Signed-off-by: Shintaro Sakoda <[email protected]>

Added ellipse diagnostics to ekf

46dcb09

Signed-off-by: Shintaro Sakoda <[email protected]>

SakodaShintaro added run:build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) tag:run-clang-tidy-differential labels Jun 26, 2024

SakodaShintaro self-assigned this Jun 26, 2024

github-actions bot added the component:localization Vehicle's position determination in its environment. (auto-assigned) label Jun 26, 2024

SakodaShintaro mentioned this pull request Jun 26, 2024

feat(ekf_localizer): add covariance ellipse diagnostics autowarefoundation/autoware_launch#1041

Merged

4 tasks

SakodaShintaro added 4 commits June 26, 2024 18:38

Removed an unnecessary parenthesis

4ef40be

Signed-off-by: Shintaro Sakoda <[email protected]>

Merge branch 'main' into refactor/move_ellipse_to_ekf

ec351a2

Fixed to ellipse_scale

5939c0a

Signed-off-by: Shintaro Sakoda <[email protected]>

Fixed to ellipse_scale

f84ea0d

Signed-off-by: Shintaro Sakoda <[email protected]>

SakodaShintaro marked this pull request as ready for review June 27, 2024 00:33

SakodaShintaro requested review from anhnv3991, KYabuuchi, kminoda, Motsu-san, RyuYamamoto, TaikiYamada4, TakaHoribe, IshitaTakeshi and YamatoAndo as code owners June 27, 2024 00:33

KYabuuchi reviewed Jun 28, 2024

View reviewed changes

SakodaShintaro added 2 commits June 28, 2024 17:38

Updated ekf_diagnostics.png

e180fd4

Signed-off-by: Shintaro Sakoda <[email protected]>

Merge branch 'main' into refactor/move_ellipse_to_ekf

f0d7ad9

github-actions bot added the type:documentation Creating or refining documentation. (auto-assigned) label Jun 28, 2024

Added condition

0a31873

Signed-off-by: Shintaro Sakoda <[email protected]>

Fixed "and" to "or"

96eab49

Signed-off-by: Shintaro Sakoda <[email protected]>

KYabuuchi approved these changes Jun 28, 2024

View reviewed changes

SakodaShintaro merged commit abe8e3c into autowarefoundation:main Jul 1, 2024
30 checks passed

SakodaShintaro deleted the refactor/move_ellipse_to_ekf branch July 1, 2024 00:08

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

feat(ekf_localizer): add covariance ellipse diagnostics #7708

feat(ekf_localizer): add covariance ellipse diagnostics #7708

SakodaShintaro commented Jun 26, 2024 •

edited

Loading

github-actions bot commented Jun 26, 2024 •

edited

Loading

codecov bot commented Jun 26, 2024 •

edited

Loading

KYabuuchi left a comment •

edited

Loading

SakodaShintaro commented Jun 28, 2024

KYabuuchi left a comment

feat(ekf_localizer): add covariance ellipse diagnostics #7708

feat(ekf_localizer): add covariance ellipse diagnostics #7708

Conversation

SakodaShintaro commented Jun 26, 2024 • edited Loading

Description

Related links

How was this PR tested?

Notes for reviewers

Interface changes

Topic changes

Modifications

ROS Parameter Changes

Additions and removals

Effects on system behavior

github-actions bot commented Jun 26, 2024 • edited Loading

codecov bot commented Jun 26, 2024 • edited Loading

Codecov Report

KYabuuchi left a comment • edited Loading

Choose a reason for hiding this comment

SakodaShintaro commented Jun 28, 2024

KYabuuchi left a comment

Choose a reason for hiding this comment

SakodaShintaro commented Jun 26, 2024 •

edited

Loading

github-actions bot commented Jun 26, 2024 •

edited

Loading

codecov bot commented Jun 26, 2024 •

edited

Loading

KYabuuchi left a comment •

edited

Loading